
#include "qyFrustum.h"

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane( const float a, const float b, const float c, const float d, Plane &plane )
{
	float	f;
	double temp;
//	const float lengthSqr = a * a + b * b + c * c;/* + d * d*/;
//	const float invLength = 1.0f / sqrt( lengthSqr );

	temp = (double)(a * a + b * b + c * c);
	temp = 1.0 / sqrt(temp);
	f = (float)temp;
	plane.Set( -a * f, -b * f, -c * f, -d * f );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Near( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][2], mat[1][2], mat[2][2], mat[3][2], plane );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Far( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][3]-mat[0][2], mat[1][3]-mat[1][2], mat[2][3]-mat[2][2], mat[3][3]-mat[3][2], plane );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Left( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][3]+mat[0][0], mat[1][3]+mat[1][0], mat[2][3]+mat[2][0], mat[3][3]+mat[3][0], plane );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Right( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][3]-mat[0][0], mat[1][3]-mat[1][0], mat[2][3]-mat[2][0], mat[3][3]-mat[3][0], plane );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Top( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][3]-mat[0][1], mat[1][3]-mat[1][1], mat[2][3]-mat[2][1], mat[3][3]-mat[3][1], plane );
}

//-----------------------------------------------------------------------
QY_INLINE static void MakeFrustumPlane_Bottom( const Matrix4 &mat, Plane &plane )
{
	MakeFrustumPlane( mat[0][3]+mat[0][1], mat[1][3]+mat[1][1], mat[2][3]+mat[2][1], mat[3][3]+mat[3][1], plane );
}

//-----------------------------------------------------------------------
void Frustum::FromViewProjectionMatrix( const Matrix4 &mat )
{
	MakeFrustumPlane_Near	( mat, m_plane[0] );
	MakeFrustumPlane_Left	( mat, m_plane[1] );
	MakeFrustumPlane_Right	( mat, m_plane[2] );
	MakeFrustumPlane_Top	( mat, m_plane[3] );
	MakeFrustumPlane_Bottom	( mat, m_plane[4] );
	MakeFrustumPlane_Far	( mat, m_plane[5] );
}

//-----------------------------------------------------------------------
bool Frustum::CullAABB( const AABB &aabb ) const
{
	Vector3 maxPoint;
	float maxDot;

	for ( int i = 0; i < 6; ++i )
	{
		const Plane &curPlane = m_plane[i];
		// This is pretty much as optimal as you can
		// get for a plane vs AABB test...
		// 
		// 4 comparisions
		// 3 multiplies
		// 2 adds
		// 1 negation
		//
		// It will early out as soon as it detects the
		// bounds is outside one of the planes.
		maxPoint.x = ( curPlane.a > 0.0f ) ? aabb.GetMax().x : aabb.GetMin().x;
		maxPoint.y = ( curPlane.b > 0.0f ) ? aabb.GetMax().y : aabb.GetMin().y;
		maxPoint.z = ( curPlane.c > 0.0f ) ? aabb.GetMax().z : aabb.GetMin().z;

		maxDot = maxPoint.x * curPlane.a + maxPoint.y * curPlane.b + maxPoint.z * curPlane.c;

		if ( maxDot <= -curPlane.d )
			return false;
	}
	return true;
}

//-----------------------------------------------------------------------
bool Frustum::CullSphere( const Vector3 &origin, const float radius ) const
{
	float maxDot;

	// Note the planes are ordered left, right, near, 
	// far, top, bottom for getting early rejections
	// from the typical horizontal scene.
	for ( int i = 0; i < 6; ++i )
	{
		const Plane &curPlane = m_plane[i];

		// This is pretty much as optimal as you can
		// get for a plane vs point test...
		// 
		// 1 comparision
		// 2 multiplies
		// 1 adds
		// 1 negation
		//
		// It will early out as soon as it detects the
		// point is outside one of the planes.

		maxDot = origin.x * curPlane.a + origin.y * curPlane.b + origin.z * curPlane.c + curPlane.d;
		if ( maxDot > radius )
			return false;
	}
	return true;
}
